﻿#pragma once
#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include<vector>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include "algorithmvision.h"
#include "algorithmvision3d.h"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/ply_io.h>
#include <pcl/console/time.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/registration/ndt.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>//模型系数定义头文件
#include <pcl/filters/project_inliers.h>//投影滤波类头文件
#include <pcl/surface/convex_hull.h>
#include<pcl/visualization/pcl_plotter.h>//绘图

#include <vtkOutputWindow.h>
//#include <liblas/liblas.hpp>

//基础点云类型定义
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::PointXYZRGBA PointShowT;
typedef pcl::PointCloud<PointShowT> PointCloudShowT;

//点云范围
struct TCloudRange
{
	float fMinDistX;
	float fMaxDistX;
	float fMinDistY;
	float fMaxDistY;
	float fMinDistZ;
	float fMaxDistZ;
	TCloudRange()
	{
		fMinDistX = -1;
		fMaxDistX = 1;
		fMinDistY = -1;
		fMaxDistY = 1;
		fMinDistZ = -1;
		fMaxDistZ = 1;
	}
	TCloudRange(float _fMinDistX, float _fMaxDistX, float _fMinDistY, float _fMaxDistY, float _fMinDistZ, float _fMaxDistZ)
	{
		fMinDistX = _fMinDistX;
		fMaxDistX = _fMaxDistX;
		fMinDistY = _fMinDistY;
		fMaxDistY = _fMaxDistY;
		fMinDistZ = _fMinDistZ;
		fMaxDistZ = _fMaxDistZ;
	}
};

class AlgorithmPclPackage
{
public:
    AlgorithmPclPackage();

public:
    //点云读取
    int readCloudPLY(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr* pCloudScene);

    int readCloudPCD(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr* pCloudScene);
    //读取点云，支持PLY PCD 自动区分
    int readCloud(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr* pCloudScene);

    //int readCloud(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr* pCloudScene);


    //点云保存
    int saveCloudToPLY(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudScene);//保存 点云.ply

    int saveCloudToPLY(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr pCloudScene);

    int saveCloudToPCD(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudScene);//保存 点云.pcd

    //保存点云，支持PLY PCD 自动区分
    int saveCloud(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudScene);

    int saveCloud(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr pCloudScene);

    //单位转换
    // fRatio 比率 mm->m时为0.001
    //CloudUnitRatio(CloudScene, 0.001);
    void CloudUnitRatio(pcl::PointCloud<pcl::PointXYZ>::Ptr& pCloud, float fRatio = 0.001);
    //---------------------------------点云类型转化------------------------------------//

    //点云转法向量点云
    pcl::PointCloud<pcl::Normal>::Ptr PCLCloudXYZ2Normal(pcl::PointCloud<pcl::PointXYZ>::Ptr& pCloud, int iK_nearest = 20);

    //点云转法向量点云
    pcl::PointCloud<pcl::PointXYZLNormal>::Ptr PCLCloudXYZ2XYZNormal(pcl::PointCloud<pcl::PointXYZ>::Ptr& pCloud, int iK_nearest = 20);

    //---------------------------------点云显示------------------------------------//
    //调试用
    //显示点云 程序阻断，当关闭点云窗口时程序继续
    void ShowCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pcloud, const std::string& sViewerName = "Cloud");

    //例程：显示点云的最小外接及位姿
    void ShowCloudAndOBB(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pcloud, const std::string& sViewerName = "Cloud");
    void ShowCloudAndOBB(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pcloud, const pcl::PointXYZ& OBBPosition, const Eigen::Matrix3f& OBBRotationalMatrix, const Eigen::Vector3f& OBBSize, const std::string& sViewerName = "Cloud");

    //开一个线程显示点云 程序不受影响， 只能调用一次
    //ShowCloudUseThread(pclCloudBox);
    //Sleep(10000);
    void ShowCloudUseThread(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pcloud, const std::string& sViewerName = "Cloud");



    //---------------------------------利用独立线程VTK显示点云------------------------------------//
    //在一个线程中显示点云并分离线程
    void ShowCloudWithThreadDetach(PointCloudT::Ptr& pCloud, std::string sCloudName = "Cloud");
    //在一个线程中显示点云并等待线程结束
    void ShowCloudWithThreadJoin(PointCloudT::Ptr& pCloud, std::string sCloudName = "Cloud");

    //---------------------------------滤波------------------------------------//
    //剪切一定范围的点云
    //	CropCloudRange(m_cloudScene_CamA, fMinDistX, fMaxDistX, fMinDistY, fMaxDistY, fMinDistZ, fMaxDistZ, m_cloudScene_CamA);
    //pCloud输入点云
    //fMin fMax xyz的剪切范围
    //pCropCloud剪切后输出点云
    void CropCloudRange(PointCloudT::Ptr& pCloud, float fMinDistX, float fMaxDistX, float fMinDistY, float fMaxDistY, float fMinDistZ, float fMaxDistZ, PointCloudT::Ptr& pCropCloud);
    void CropCloudRange(PointCloudT::Ptr& pCloud, TCloudRange tCloudRange, PointCloudT::Ptr& pCropCloud);

    //手动判断范围剪切点云 比原始的点云函数运行要快
    void CropCloudRange2(PointCloudT::Ptr& pCloud, float fMinDistX, float fMaxDistX, float fMinDistY, float fMaxDistY, float fMinDistZ, float fMaxDistZ, PointCloudT::Ptr& pCropCloud);
    void CropCloudRange2(PointCloudT::Ptr& pCloud, TCloudRange tCloudRange, PointCloudT::Ptr& pCropCloud);

    //去除无效点云
    void removeNaNCloud(PointCloudT::Ptr& pNaNCloud, PointCloudT::Ptr& pCloud);

    //采样滤波
    //SamplingFilter(m_cloudScene_CamA, leaf, leaf, leaf, m_cloudScene_CamA);
    //pCloud输入点云
    //fLeaf xyz的体素大小
    //pSamplingCloud采样后输出点云
    void SamplingFilter(PointCloudT::Ptr& pCloud, float fLeafX, float fLeafY, float fLeafZ, PointCloudT::Ptr& pSamplingCloud);

    //半径滤波
    //RadiusFilter(pCloud, 0.002,5,pRadiusFilterCloud);//2mm内需要有5个点
    //pCloud输入点云
    //fRadius指定半径 0.002 ：2mm
    //fMinNeighbors 半径内需要的最小近邻数 如5
    //pRadiusFilterCloud半径滤波后输出点云
    void RadiusFilter(PointCloudT::Ptr& pCloud, float fRadius, float fMinNeighbors, PointCloudT::Ptr& pRadiusFilterCloud);

    //统计滤波  (比较耗时)
    //StatisticalFilter(pCloud,50,1.0,pCloud) //统计50个点，移除1个标准差外的点
    //pCloud输入点云
    //iMeanK 统计的点云数量
    //fStddevThresh 标准差阈值
    //pStatisticalFilterCloud统计滤波后输出点云
    void StatisticalFilter(PointCloudT::Ptr& pCloud, int iMeanK, float fStddevThresh, PointCloudT::Ptr& pStatisticalFilterCloud);


    //添加高斯噪声 均值为mu，标准差为sigma
    void AddGaussNoise(PointCloudT::Ptr& pCloud, float fMu, float fSigma, PointCloudT::Ptr& pGaussNoiseCloud);

    //剪切特定方向点云 //根据向量方向进行裁剪
    void CropCloudNormalRange(PointCloudT::Ptr& pCloud, float fNormalAngleRange, PointCloudT::Ptr& pNormalRangeCloud, char Axis = 'z');



    //---------------------------------聚类------------------------------------//
    //欧式聚类提取 （欧几里得聚类提取）
    //pCloud输入点云
    //fClusterTolerance最小距离 单位m  0.002代表2mm
    //iMinClusterSize 聚类点云最少点 如50个点
    //iMaxClusterSize 聚类点云最多点 如99999999999个点
    //pvCloudPtr 聚类出的所有点云
    void EuclideanClusterExtractionCloud(const PointCloudT::Ptr& pCloud, float fClusterTolerance, int iMinClusterSize, int iMaxClusterSize, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>* pvCloudPtr);


    //点云拷贝
    //PointCloudT::Ptr pclCloudBox = cvCloud2PCLCloud(cvCloudBox);
    //PointCloudT::Ptr pclCloudBox2(new pcl::PointCloud<pcl::PointXYZ>);
    //pcl::copyPointCloud<pcl::PointXYZ, pcl::PointXYZ>(*pclCloudBox, *pclCloudBox2);

    //获取最大点云 点数最多的点云
    void getMaxCloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloudPtr, pcl::PointCloud < pcl::PointXYZ>::Ptr* pCloudPtr);
    pcl::PointCloud < pcl::PointXYZ>::Ptr getMaxCloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloudPtr);
    //---------------------------------投影------------------------------------//
    //变换点云
    void TransformPointCloud(PointCloudT::Ptr& pCloud, Eigen::Matrix4f M4fTransformMatrix, PointCloudT::Ptr& pTranCloud);

    //投影点云到一个平面
    //投影点云到z=0（X-Y）平面
    //ProjectOntoPlane(m_cloudScene_CamA, 0, 0, 1, 0, m_cloudScene_CamA);
    //pCloud输入点云
    //fa fb fc fd 平面系数方程系数 ax+by+cz+d=0
    //pProjectCloud投影后输出点云
    void ProjectOntoPlane(PointCloudT::Ptr& pCloud, float fa, float fb, float fc, float fd, PointCloudT::Ptr& pProjectCloud);


    //PCL点云转Opencv图像
    //cv::Mat PointCloudImage;
    //CloudPoint2Mat(m_cloudScene_CamA, fResolution, fRangeX, fRangeY, fOffsetX, fOffsetY, &PointCloudImage, 0);
    //fResolution分辨率 一个像素多少m
    //fRangeX x方向范围m
    //fRangeY y方向范围m
    //fOffsetX x平移范围m
    //fOffsetY y平移范围m
    //输出图像pPointCloudImage
    //iStatisticalMode统计模式
    //0:严格点云统计（计算像素对应空间位置一定分辨率附近的点云数，输入点云事先需要投影到Z=0平面，时间参考30ms）
    //1:快速点云统计（忽略点云z,将每个点云放置到对应像素中计数，部分像素无点云，时间参考<1ms）
    void CloudPoint2Mat(PointCloudT::Ptr& pCloud, float fResolution, float fRangeX, float fRangeY, float fOffsetX, float fOffsetY, cv::Mat* pPointCloudImage, int iStatisticalMode = 0);

    //PCL点云转Opencv图像 
    //矫正xy的方向
    //XY平面点云转opencv图像
    void CloudXYPlane2Mat(PointCloudT::Ptr& pCloud, float fResolution, float fRangeX, float fRangeY, float fOffsetX, float fOffsetY, cv::Mat* pPointCloudImage, int iStatisticalMode = 0);
#define CloudPoint2MatNew CloudXYPlane2Mat;

    //PCL点云转点数组
    std::vector<cv::Point3f> CloudPoint2vP3f(const pcl::PointCloud<pcl::PointXYZ>::Ptr& Cloud);

    //投影点云转深度图
    cv::Mat ProjectionCloud2DepthImage(const pcl::PointCloud<pcl::PointXYZ>::Ptr& Cloud, const int& iImageRows, const int& iImageClos, const cv::Mat& matCameraMatrix, const cv::Mat& matDistCoeffs, bool bswapXY = false);

    //投影雷达转深度图
    cv::Mat ProjectionLidar2DepthImage(const pcl::PointCloud<pcl::PointXYZ>::Ptr& Cloud, const int& iImageRows, const int& iImageClos, const cv::Mat& matCameraMatrix,const cv::Mat& matLidarExtrinsic, const cv::Mat& matDistCoeffs, bool bswapXY = false);


    //PCL点云转Opencv点云   
    //cv::Mat PCLCloud2cvCloud(PointCloudT::Ptr& pCloud);

    //把点投影到2D平面上
    //PCL点云转Opencv点云
    cv::Mat PCLCloud2cvCloud(PointCloudT::Ptr& pCloud, const int& iImageRows, const int& iImageClos, const cv::Mat& matCameraMatrix, const cv::Mat& matDistCoeffs, cv::Mat* DepthMask = NULL);
    cv::Mat PCLCloud2cvCloudMask(PointCloudT::Ptr& pCloud, const int& iImageRows, const int& iImageClos, const cv::Mat& matCameraMatrix, const cv::Mat& matDistCoeffs);


    //opencv点云转PCL点云
    PointCloudT::Ptr cvCloud2PCLCloud(const cv::Mat& cvCloud, float fUnitConversionFactor = 0.001);

#define Mat2PCL cvCloud2PCLCloud

    //---------------------------------计算特定值------------------------------------//
    //计算某个x-y范围z的平均值
    //pCloud 旋转矫正剪切后的输入点云
    //fOffsetX x平移范围m
    //fOffsetY y平移范围m
    //fRangeX x方向范围m
    //fRangeY y方向范围m
    //fMinZ z最小值
    // fMaxZ z最大值
    //pfMeanZ Z的平均值指针m
    void CalculateMeanZ(PointCloudT::Ptr& pCloud, float fOffsetX, float fOffsetY, float fRangeX, float fRangeY, float* pfMeanZ, int* piRangePointNum = NULL);
    void CalculateMeanZ(PointCloudT::Ptr& pCloud, float fOffsetX, float fOffsetY, float fRangeX, float fRangeY, float fMinZ, float fMaxZ, float* pfMeanZ, int* piRangePointNum);


    //计算点云的中值点 Z中值对应的点
    pcl::PointXYZ CalculateCloudZMidValuePoint(PointCloudT::Ptr& pCloud);

    //提取点云内点
    //pCloud点云
    //inliers索引
    //pCloudExtract 索引对应的点
    void ExtractIndicesCloud(const PointCloudT::Ptr& pCloud, const pcl::PointIndices::Ptr& inliers, PointCloudT::Ptr* pCloudExtract);

    //---------------------------------点云分析------------------------------------//
    //点云拟合平面
    //pCloud输入点云
    //fMaxDist 平面点最大距离  单位m
    //pcoefficients平面系数
    //pCloudPlane平面点云内点
    ////平面分割
    //pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudPlane;
    //pcl::ModelCoefficients::Ptr pcoefficients;
    //PlaneSegmentation(Cloud2, 0.01, &pcoefficients, &pCloudPlane);
    //ShowCloud(pCloudPlane);
    void PlaneSegmentation(PointCloudT::Ptr& pCloud, float fMaxDist, pcl::ModelCoefficients::Ptr* pcoefficients = NULL, PointCloudT::Ptr* pCloudPlane = NULL);
    //平面系数转欧拉角
    //pfAlpha pfBeta pfGamma 分别为绕XYZ旋转的角度  pfAlpha-90绕Y轴的角度 pfBeta-90绕X轴的角度 
    // pfDist 原点到平面的距离
    // //计算平面角度
    //float fAlpha, fBeta, fGamma, fDistance;
    //PlaneCoefficients2Euler(pcoefficients, &fAlpha, &fBeta, &fGamma, &fDistance);
    void PlaneCoefficients2Euler(const pcl::ModelCoefficients::Ptr& coefficients, float* pfAlpha = NULL, float* pfBeta = NULL, float* pfGamma = NULL, float* pfDist = NULL);


    //计算平面点云位姿 利用最小包围盒 
    ////平面点云最小包围盒及位姿
    //pcl::PointXYZ OBBPosition;
    //Eigen::Matrix3f OBBRotationalMatrix;
    //Eigen::Vector3f OBBSize;
    //pcl::PointXYZ  MassCenter;
    //PlaneOBB(pCloudPlane, &OBBPosition, &OBBRotationalMatrix, &OBBSize, &MassCenter);
    ////角度转换显示
    //Eigen::Vector3f eulerAngle = OBBRotationalMatrix.eulerAngles(2, 1, 0);
    ////std::cout << "eulerAngle =\n" << eulerAngle << endl;
    //double dXA = -eulerAngle[2] * 180 / PI;
    //double dYA = -eulerAngle[1] * 180 / PI;
    //double dZA = -eulerAngle[0] * 180 / PI;
    ////显示点云和包围盒
    ////ShowCloudAndOBB(pCloudPlane,"1");
    //ShowCloudAndOBB(pCloudPlane, OBBPosition, OBBRotationalMatrix, OBBSize, "1");
    void PlaneOBB(const PointCloudT::Ptr& pCloud, pcl::PointXYZ* pOBBPosition = NULL, Eigen::Matrix3f* pOBBRotationalMatrix = NULL, Eigen::Vector3f* pOBBSize = NULL, pcl::PointXYZ* pMassCenter = NULL);

    //点云最小包围盒
    void CloudOBB(const PointCloudT::Ptr& pCloud, pcl::PointXYZ* pOBBPosition = NULL, Eigen::Matrix3f* pOBBRotationalMatrix = NULL, Eigen::Vector3f* pOBBSize = NULL, pcl::PointXYZ* pMassCenter = NULL);

    //点云AABB包围盒
    void CloudAABB(const PointCloudT::Ptr& pCloud, pcl::PointXYZ* maxpoint, pcl::PointXYZ* minPoint);

    void CloudAABB(const PointCloudT::Ptr& pCloud, Eigen::Vector3f* pAABBSize, pcl::PointXYZ* pMassCenter);

    //估计边界 边界提取
    int estimateBorders(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, float re, float reforn, pcl::PointCloud<pcl::PointXYZ>::Ptr* pcloud_boundary);

    //凸包计算 凸包由最外围的点云构成
    void ConvexHull(PointCloudT::Ptr& pCloud, PointCloudT::Ptr* pCloudConvexHull);

    //-----------绘图------------//
    //	//定义一个plottter类
    //pcl::visualization::PCLPlotter* plotter = new pcl::visualization::PCLPlotter();
    ////设置一些属性
    //plotter->setShowLegend(true);
    ////生成对应点
    //std::vector<double> vdY1 = { 1,2,23,333,444,22,222,333,222 };
    //std::vector<double> vdY2 = { 13,2,23,333,44,22,222,333,222 };
    //std::vector<double> vdIndex;
    //for (int ii = 0; ii < vdY1.size(); ii++)
    //{
    //	vdIndex.push_back(ii);
    //}
    ////添加绘制的数据
    //plotter->addPlotData(&vdIndex[0], &vdY1[0], vdY1.size(), "Y1");
    //plotter->addPlotData(&vdIndex[0], &vdY2[0], vdY2.size(), "Y2");
    //
    ////设置Y轴区域
    //plotter->setYRange(-10, 1000);
    //
    ////一直显示
    ////plotter->plot();
    //
    ////显示3秒
    //plotter->spinOnce(30000);
    //plotter->clearPlots();


    //绘制一维矢量
    void plotvd(const std::vector<double>& vdY1, const std::string& name = "Y");

    //绘制多个同等长度的矢量
    void plotvd(const std::vector<double>& vdY1, const std::string& Y1name, const std::vector<double>& vdY2, const std::string& Y2name);
    void plotvd(const std::vector<double>& vdY1, const std::string& Y1name, const std::vector<double>& vdY2, const std::string& Y2name, const std::vector<double>& vdY3, const std::string& Y3name);
    void plotvd(const std::vector<double>& vdY1, const std::string& Y1name, const std::vector<double>& vdY2, const std::string& Y2name, const std::vector<double>& vdY3, const std::string& Y3name, const std::vector<double>& vdY4, const std::string& Y4name);
    //绘制多个一维向量 不限个数
    //plotvd(2, vvdAGVCurrentLine_Point_T[1], std::string("YOffset"), vvdAGVCurrentLine_Point_T[2], std::string("AOffset"));
    //plotvd(3, vvdAGVCurrentLine_Point_T[5], "x", vvdAGVCurrentLine_Point_T[6], "y", vvdAGVCurrentLine_Point_T[7], "A");//绘制一维点
    void plotvd(int count, ...);

    //绘制二维点

    void plotXY(const std::vector<double>& vdX1, const std::vector<double>& vdY1, const std::string& name = "Y");
    void plotXY(const std::vector<double>& vdX1, const std::vector<double>& vdY1, const std::string& Y1name, const std::vector<double>& vdX2, const std::vector<double>& vdY2, const std::string& Y2name);

    //绘制多个二维点 可变参数 可变形参
    //plotXY(2, vvdAGVCurrentLine_Point_T[5], vvdAGVCurrentLine_Point_T[6], "x-y", vvdAGVCurrentLine_Point_T[5], vvdAGVCurrentLine_Point_T[7], "x-A");
    void plotXY(int count, ...);

    inline void viewPCL(const pcl::PointCloud<pcl::PointXYZ>& pointCloud);

    inline void viewPCL(const pcl::PointCloud<pcl::PointXYZRGB>& colorPointCloud);

    PointCloudT PlanRotate(PointCloudT input, double A, double B, double C, PointCloudT& transformed_cloud);

    void viewerOneOff(pcl::visualization::PCLVisualizer& viewer);

    void viewerPsycho(pcl::visualization::PCLVisualizer& viewer);

    int readCloudPLY(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr* pCloudScene);

    int readCloudPCD(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr* pCloudScene);

    int readCloud(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr* pCloudScene);

    int saveCloudToPCD(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr pCloudScene);

 private:
     AlgorithmVision algorithmVision;
     AlgorithmVision3D algorithmVision3D;
};
